from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument
from launch.launch_description_sources import PythonLaunchDescriptionSource

from ament_index_python.packages import get_package_share_directory
from launch.conditions import IfCondition
import os
from launch.actions import IncludeLaunchDescription
from launch.substitutions import LaunchConfiguration


def generate_launch_description():
    go2_desc_pkg = get_package_share_directory("go2_description")
    go2_driver_pkg = get_package_share_directory("go2_driver_py")

    use_rviz = DeclareLaunchArgument(
        name="use_rviz",
        default_value="true"
    )
    
    return LaunchDescription([
        use_rviz,
        #rviz集成URDF
        IncludeLaunchDescription(
            launch_description_source=PythonLaunchDescriptionSource(
                launch_file_path=os.path.join(go2_desc_pkg,"launch","display.launch.py")
            ),
            launch_arguments=[("use_join_state_publisher","false")]
        ),
            
        Node(
            package="rviz2",
            executable="rviz2",
            arguments=["-d", os.path.join(go2_desc_pkg, "rviz", "go2.rviz")],
            condition=IfCondition(LaunchConfiguration("use_rviz"))
        ),
        # Node(
        #     package="go2_twist_bridge_py",
        #     executable="twist_bridge"
        # ),

        #urdf中雷达坐标系名称对应
        # Node(
        #     package="tf2_ros",
        #     executable="static_transform_publisher",
        #     arguments=["0", "0", "0", "0", "0", "0", "radar", "utlidar_lidar"]
            
        # ),

        #雷神N10P雷达接入
        # Node(
        #     package="tf2_ros",
        #     executable="static_transform_publisher",
        #     arguments=["0", "0", "0", "0", "0", "0", "radar", "laser"]
            
        # ),
        # Node(
        #     package="tf2_ros",
        #     executable="static_transform_publisher",
        #     arguments=["0", "0", "0", "0", "0", "0", "base", "utlidar_lidar"]
            
        # ),

        Node(
            package="go2_driver_py",
            executable="driver",
            parameters=[
                os.path.join(go2_driver_pkg, "params", "go2_driver.yaml")
            ]
        ),
    ])